#include "spi_controller.h"
#include "flash_controller.h"
#include "interrupt_controller.h"
#include "os_dev.h"
#include "os_io.h"
#include "ksem.h"
#include "sh_mem.h"
#include "string.h"

#include "os_mem.h"

static flash_config_t *flash_config = NULL;

void dump_flash_regs(void)
{
    uint32_t base = FLASH_BASE;
 //   uint16_t i = 0;

printf("\n\n");
printf("SPI_GCR0: 0x%x\n", SPI_GCR0(base));
printf("SPI_GCR1: 0x%x\n", SPI_GCR1(base));
printf("SPI_INT0: 0x%x\n", SPI_INT0(base));
printf("SPI_LVL: 0x%x\n", SPI_LVL(base));
printf("SPI_FLG: 0x%x\n", SPI_FLG(base));
printf("SPI_PC0: 0x%x\n", SPI_PC0(base));
printf("SPI_PC1: 0x%x\n", SPI_PC1(base));
printf("SPI_PC2: 0x%x\n", SPI_PC2(base));
printf("SPI_PC3: 0x%x\n", SPI_PC3(base));
printf("SPI_PC4: 0x%x\n", SPI_PC4(base));
printf("SPI_PC5: 0x%x\n", SPI_PC5(base));
printf("SPI_RES1: 0x%x\n", SPI_RES1(base));
printf("SPI_RES2: 0x%x\n", SPI_RES2(base));
printf("SPI_RES3: 0x%x\n", SPI_RES3(base));
printf("SPI_DAT0: 0x%x\n", SPI_DAT0(base));
printf("SPI_DAT1: 0x%x\n", SPI_DAT1(base));
printf("SPI_BUF: 0x%x\n", SPI_BUF(base));
printf("SPI_EMU: 0x%x\n", SPI_EMU(base));
printf("SPI_DELAY: 0x%x\n", SPI_DELAY(base));
printf("SPI_DEF: 0x%x\n", SPI_DEF(base));
printf("SPI_FMT0: 0x%x\n", SPI_FMT0(base));
printf("SPI_FMT1: 0x%x\n", SPI_FMT1(base));
printf("SPI_FMT2: 0x%x\n", SPI_FMT2(base));
printf("SPI_FMT3: 0x%x\n", SPI_FMT3(base));
printf("SPI_VEC0: 0x%x\n", SPI_VEC0(base));
printf("SPI_VEC1: 0x%x\n", SPI_VEC1(base));

/*
printf("\n\nbytes rx: 0x%x\n", flash_config->bytes_received);
for (i = 0; i < flash_config->bytes_received; i++) {
   printf("RX: 0x%x\n", flash_config->receive_buffer[i]);
}

printf("\n\nbytes tx: 0x%x\n", flash_config->bytes_transmitted);
for (i = 0; i < flash_config->bytes_transmitted; i++) {
   printf("RX: 0x%x\n", flash_config->transmit_buffer[i]);
}
*/



}

#ifdef FLASH_OPERATIONAL
void start_flash_operation(flash_config_t *flash_config)
{
    //uint8_t value = 0;
    uint32_t base = FLASH_BASE;
    //bool transmit = FALSE;
    uint16_t rx_count = 0;
    uint16_t i = 0;

    //SPI_GCR1(base) |= 0x01000000;

    // Disable SPI
    SPI_GCR1(base) &= 0xFEFFFFFF;

    flash_config->bytes_received = 0;
    flash_config->bytes_transmitted = 0;
    flash_config->bytes_ignored = 0;

/*
    if (flash_config->receive_count) {
        // We are supposed to receive data
	// Don't drive CS high after TX complete
	//SPI_DEF(base) = 0x00;
	receive = TRUE;
    }
*/

    if (flash_config->transmit_count) {

       //transmit = TRUE;

        // ISR disables TX interrupt on TX complete
	// Re-enable it here
	//SPI_INT0(base) |= 0x00000200; // @todo: Enable the interrupt here

        // Pop first TX word from buffer. 
	// It shall be sent from this routine. The
	// ISR shall take care of the rest.
        //value = flash_config->transmit_buffer[0];
	//flash_config->transmit_count--;
	//flash_config->bytes_transmitted = 1;
    }

    if (flash_config->receive_count) {
        SPI_DAT1(base) = 0x10FE0000; // Keep CS_LOW 
	flash_config->ignore_rx_byte = TRUE;
	rx_count = flash_config->receive_count;
    }


    //rx_count = (uint16_t)SPI_DAT1(base);
    //dump_flash_regs();
    //printf("Value to transmit: 0x%x and SPI_DAT1 is: 0x%x\n", value, SPI_DAT1(base));

    // Enable SPI
    // It should trigger TX empty interrupt to start transmission
    SPI_GCR1(base) |= 0x01000000;

/*
    if (transmit) {
        printf("Transmitting to flash with value: 0x%x\n", value);
	//SPI_DAT1_3(base) = 0x10;  // Set CS low
	//SPI_DAT1_0(base) = value;
	SPI_DAT1(base) = (0x10FE0000 | value);
	//SPI_DAT0(base) = 0;

    }
*/

    sem_lock(&flash_config->client_lock);

    if (rx_count) {
        printf("Received 0x%x bytes\n", flash_config->bytes_received);
        for (i = 0; i < flash_config->bytes_received; i++)
        {
            if ((i & 0x3) == 0) { printf("\n"); }
	    printf("0x%x ", flash_config->receive_buffer[i]);
        }
	printf("\n\n");
    }

    return;
}
#endif



void test_flash(void)
{
    //uint16_t i = 0;
    uint8_t *tx_buffer = calloc(1, 10);
    uint8_t *rx_buffer = calloc(1, 500);

    printf("Testing flash\n");
    if (!tx_buffer || !rx_buffer) {
        printf("Allocation of buffer failed\n");
	return;
    }
    //printf("PINMUX value is: 0x%x\n", *(uint32_t *)0x01C14134);
    flash_config->transmit_count = 1;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 7;
    tx_buffer[0] = 0x9F;

    //start_flash_operation();
/*
    // Read status register
    printf("Reading status register\n");
    flash_config->transmit_count = 1;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 1;
    tx_buffer[0] = 0x05;
    start_flash_operation();

    // Write enable
    printf("Write latch enable\n");
    flash_config->transmit_count = 1;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 0;
    tx_buffer[0] = 0x06;
    start_flash_operation();

    // Write status register
    flash_config->transmit_count = 2;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 0;
    tx_buffer[0] = 0x01;
    tx_buffer[1] = 0x1c;
    start_flash_operation();
    printf("Write enabled\n");

    // Read status register again
    printf("Reading status register\n");
    flash_config->transmit_count = 1;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 1;
    tx_buffer[0] = 0x05;
    rx_buffer[0] = 0xff;
    start_flash_operation();


    // Read flash data
    printf("Reading flash data\n");
    flash_config->transmit_count = 4;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 100;
    tx_buffer[0] = 0x03;
    tx_buffer[1] = 0x08;
    tx_buffer[2] = 0x00;
    tx_buffer[3] = 0x00;
    start_flash_operation();

    // Read Electronic signature
    printf("Reading electronic signature\n");
    flash_config->transmit_count = 4;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 5;
    tx_buffer[0] = 0xAB;
    tx_buffer[1] = 0x08;
    tx_buffer[2] = 0x00;
    tx_buffer[3] = 0x00;
    start_flash_operation();
    // Read flash data
    printf("Reading flash data\n");
    flash_config->transmit_count = 4;
    flash_config->transmit_buffer = tx_buffer;
    flash_config->receive_buffer = rx_buffer;
    flash_config->receive_count = 100;
    tx_buffer[0] = 0x03;
    tx_buffer[1] = 0x08;
    tx_buffer[2] = 0x00;
    tx_buffer[3] = 0x00;
    start_flash_operation();

*/
    //printf("Bytes left to receive: 0x%x\n", flash_confaig->receive_count);
    ///printf("0x%x, 0x%x, 0x%x\n", rx_buffer[0], rx_buffer[1], rx_buffer[2]);

    //dump_flash_regs();

    return;
}

#ifdef FLASH_OPERATIONAL
err_t flash_write_data(flash_config_t *flash_config, flash_tx_buffer_t *flash_buffer, uint16_t bytes, uint32_t addr)
{
   err_t err_code;
   uint8_t *buff_header = NULL;
   uint8_t *tx_buffer = &flash_header->tx_header;
    
   err_code = sem_lock(&flash_config->flash_lock);
   if (err_code != ERR_SUCCESS) {
       return err_code;
   }

   buff_header = (uint8_t *)calloc(1, 10);  // shared memory, needs access from ISR
   if (!buff_header) {
       err_code = ERR_NO_MEM;
       sem_unlock(&flash_config->flash_lock);
       return err_code;
   }

   flash_enable_write(flash_config, buff_header);

   addr = addr & 0xFFFFFF;  // 3 byte addresses

   flash_config->transmit_count = bytes + XXX;
   tx_buffer[X] = YYYY;
   tx_buffer[X+1] = (addr >> 16);
   tx_buffer[X+2] = (addr >> 8) & 0xFF;
   tx_buffer[X+3] = addr & 0xFF;
   flash_config->transmit_buffer = &tx_buffer[X];

   flash_config->receive_buffer = NULL;
   flash_config->receive_count = 0;

   start_flash_operation(flash_config);

   sem_lock(&flash_config->client_lock);  // Wait for ISR operations completion

   return ERR_SUCCESS;



err_t flash_enable_write(flash_config_t *flash_config, uint8_t *tx_buffer)
{
   err_t err_code;
   uint8_t extra_data_bytes = 5; // @todo: recalibrate
   uint8_t *tx_buffer = NULL;



   addr = addr & 0xFFFFFF;  // 3 byte addresses

   flash_config->transmit_count = 4;
   tx_buffer[0] = 0x03;
   tx_buffer[1] = (addr >> 16);
   tx_buffer[2] = (addr >> 8) & 0xFF;
   tx_buffer[3] = addr & 0xFF;
   flash_config->transmit_buffer = tx_buffer;

   flash_config->receive_buffer =  buffer;
   flash_config->receive_count = bytes;

   start_flash_operation(flash_config);

   sem_lock(&flash_config->client_lock);  // Wait for ISR operations completion

   return ERR_SUCCESS;
}

// buffer: It should be in shared memory, to which the ISR can write.
err_t flash_read_data(flash_config_t *flash_config, uint32_t addr, uint16_t bytes, uint8_t *buffer)
{
   err_t err_code;
   uint8_t extra_data_bytes = 5; // @todo: recalibrate
   uint8_t *tx_buffer = NULL;

   err_code = sem_lock(&flash_config->flash_lock);
   if (err_code != ERR_SUCCESS) {
       return err_code;
   }

   tx_buffer = (uint8_t *)calloc(1, 10);  // shared memory, needs access from ISR
   if (!tx_buffer) {
       err_code = ERR_NO_MEM;
       sem_unlock(&flash_config->flash_lock);
       return err_code;
   }

   addr = addr & 0xFFFFFF;  // 3 byte addresses

   flash_config->transmit_count = 4;
   tx_buffer[0] = 0x03;
   tx_buffer[1] = (addr >> 16);
   tx_buffer[2] = (addr >> 8) & 0xFF;
   tx_buffer[3] = addr & 0xFF;
   flash_config->transmit_buffer = tx_buffer;

   flash_config->receive_buffer =  buffer;
   flash_config->receive_count = bytes;

   start_flash_operation(flash_config);

   sem_lock(&flash_config->client_lock);  // Wait for ISR operations completion

   return ERR_SUCCESS;
}

#endif



   
void spi1_interrupt_handler(void)
{
    uint32_t base = FLASH_BASE;
    uint32_t interrupt;
    uint16_t temp = 0;
    uint8_t value = 0;
    //static uint8_t count = 0;
    static uint8_t tx_count = 0;
    static uint8_t rx_count = 0;

 //   uint32_t status = 0;
 //   bool check_again = TRUE;

    // SPI1 interrupt
    ARM_INTC_SICR = AINTC_SPI1_INTERRUPT;		// Clear interrupt

    interrupt = SPI_VEC1(base);
    interrupt = interrupt >> 1;

    while (interrupt != 0) {
    //printf("Interrupt is: 0x%x\n", interrupt);
    
    if (interrupt == 0x14) {
        tx_count++;
        // Transmit buffer empty register
	if (flash_config->transmit_count) {
	    SPI_DAT0(base) = flash_config->transmit_buffer[flash_config->bytes_transmitted++];
	    flash_config->transmit_count--;
	    printf("TX fill 0x%x\n", SPI_DAT0(base));
	} else {
	        //flash_config->bytes_ignored++;
	        if (flash_config->receive_count) {
		    SPI_DAT0(base) = 0x00;  // Keep running TX with dummy values to drive the clock
		} else {
	            // Disable interrupt
		    SPI_INT0(base) &= 0xFFFFFDFF;
		}
	}
    } else if (interrupt == 0x12) {
        // Receive buffer Full
	printf("RX fill\n");
	
//	if (flash_config->bytes_ignored < 2 && FALSE) {   // @todo: change back to ignore_rx_byte
	if ("SEE what to do here") {
	    //flash_config->ignore_rx_byte = FALSE;
	    value = SPI_BUF(base) & 0xFF;
	} else {

    	    rx_count++;
    	    if (flash_config->receive_count) {
    	        value = SPI_BUF(base) & 0xFF;
    	        flash_config->receive_buffer[flash_config->bytes_received++] = value;
    	        flash_config->receive_count--;
		//if (flash_config->transmit_count) { printf("Transmit data still left\n"); }
    	        //printf("Value received is: 0x%x, 0x%x\n", value, flash_config->receive_count);
    	    } else {
    	        // Unsolicited receive. Discard it.
    	        temp = SPI_BUF(base) & 0xFFFF;
    	        //printf("Unsolicited data received: 0x%x\n", temp);
    	    }
	}
    } else if (interrupt == 0x13) {
        // Receive buffer overrun
	// Should not happen. Just shout!!
	printf("RX data overrun: 0x%x\n", temp);
    } else if (interrupt == 0x11) {
        // Some error interrupt
	printf("Some error occured in SPI1 flash: 0x%x\n", SPI_FLG(base));
	SPI_FLG(base) = 0x1F;
    }

    interrupt = SPI_VEC1(base);
    interrupt = interrupt >>1;
    }

    if (!flash_config->transmit_count && !flash_config->receive_count) {
        // flash operations complete
	//SPI_GCR1(base) &= 0xFEFFFFFF;  // disable flash transfers
	//count = 0;
	// Drive CS high
	SPI_DAT1_3(base) &= 0xEF;
	unlock_ksem((uint32_t)&flash_config->client_lock);
	//printf("All values received: tx:0x%x, rx:0x%x\n", tx_count, rx_count);
    }
}


static err_t flash_config_mem_init(void)
{
    dev_handle_t *dev_handle;
    err_t err_code;
    uint16_t size = sizeof(flash_config_t);
    uint8_t *name = (uint8_t *)FLASH_CONFIG_MEM;
    shm_open_mode_t open_mode = SHM_CREATE_ALWAYS;

    dev_handle = (dev_handle_t *)calloc(1, sizeof(dev_handle_t));
    if (!dev_handle) { return ERR_NO_MEM; }

    dev_handle->dev_info.name = calloc(1, strlen(name)+1);
    if (!dev_handle->dev_info.name) {
        free(dev_handle);
	return ERR_NO_MEM;
    }

    strcpy((uint8_t *)dev_handle->dev_info.name, name);
    dev_handle->dev_info.type = DEVICE_TYPE_SHMEM;
    dev_handle->dev_info.size  = size;
    dev_handle->open_mode = open_mode;
    dev_handle->access_info.permissions = DEVICE_MODE_WRITE;

    open_device((uint32_t)dev_handle);
  
    err_code = dev_handle->err_code;
    if (dev_handle->err_code == ERR_SUCCESS) {
	flash_config = (flash_config_t *)dev_handle->dev_info.address;
	err_code = sem_create2(&flash_config->flash_lock, true, false);
	if (err_code != ERR_SUCCESS) {
	    close_device((uint32_t)dev_handle);
	} else {
	    err_code = sem_create2(&flash_config->client_lock, true, true);
	    if (err_code != ERR_SUCCESS) {
	        close_device((uint32_t)dev_handle);
	    }
	}
    } 
    
    if (err_code != ERR_SUCCESS) {
        free((void *)dev_handle->dev_info.name);
	free((void *)dev_handle);
    }

    return err_code;
}

err_t flash_init(void)
{
    uint32_t base = FLASH_BASE;
    err_t err_code;

    // Reset/Un-reset the controller
    SPI_GCR0(base) = 0;
    SPI_GCR0(base) = 1;

    SPI_GCR1(base) = 3; // Master mode
    SPI_PC0(base) = 0xE01; // Enable Tx/Rx/Clk and chip-select

    // chip-select during transfer = low (16:0)
    // data-format = FMT0(24-25:0)
    // chip-select deactivated at end of transfer(28:0)
    SPI_DAT1(base) = 0x00FE0000;

    // 8 bit word, clock high-inactive(17:0), no delays(18:1)
    // MSB first(20:0), no idle time b/w transmissions(24-29:0)
    // Prescalar (8-15:4 => clk/(4+1))
    SPI_FMT0(base) = 0x00040408;  

    // Configured a 0 clock-sycles delay between transmit end
    // and chip-select deactivate. Might need to be configured
    // per instruction at runtime!!
    SPI_DELAY(base) = 0x00000000; 

    // By default chip-select is driven high after transmit from master is complete
    // Its required by some instructions of M25P64 flash
    // For other instructions, SW shall have to keep chip-select low when RX data
    // follows after TX command
    SPI_DEF(base) = 0x01;  

#if 0
    // Send a dummy byte
    SPI_GCR1(base) |= 0x01000000;
    SPI_DAT0(base) = 0x9F;
    while ((SPI_FLG(base) & 0x200) == 0);
    SPI_GCR1(base) &= 0xFEFFFFFF;
#endif

    // Enable interrupts
    SPI_INT0(base) = 0x01000150; // @todo: change back to 0x01000350  
    SPI_LVL(base) = 0x358;
    enable_interrupt(AINTC_SPI1_INTERRUPT);
    
    // Enable SPI transfers
    //SPI_GCR1(base) |= 0x01000000;

    //err_code = (err_t)SPI_BUF(base);

    err_code = flash_config_mem_init();


    return err_code;
}

    
